
#include "user_pid.h"

PID_element PID[3];


void init_PID(void)
{
    init_PID_variable(&PID[Roll]);
    init_PID_variable(&PID[Pitch]);
    init_PID_variable(&PID[Yaw]);

    PID[Roll].i_gain = 0;//120;
    PID[Pitch].i_gain = 0;// 110;
    PID[Yaw].i_gain = 0;//180;
}
void init_PID_variable(PID_element *pid)
{
    pid->p_gain = 10;
    pid->p_dyn_gain = 0;
    pid->i_gain = 0;
    pid->i_dyn_gain= 0;
    pid->i_state = 0;
    pid->i_min = -10000;
    pid->i_min = 10000;
    pid->d_gain = 0;
    pid->d_dyn_gain = 0;
    pid->d_state = 0;
    pid->d_0_state = 0;
    pid->d_1_state = 0;
    pid->d_old_gyro_rate =0;
}

/* Pseudo derivative filter. */
int32_t update_PDF(PID_element *pid, int32_t RC_position,int32_t angle_position, int32_t Gyro_Rate)
{
    int32_t err_possition;
    int32_t p_term;
    int32_t i_term;
    int32_t d_term;
    int32_t d_state;
    int32_t d_summ;
    int32_t pid_summ;

    /*
    * And angle_position should be +-1800 in 0.1 deg, that means +-180 degree output of kalman filter
    * for Roll,Pitch or Yaw.
    */

    /* Level error calculation. */
    err_possition = constrain(2 * RC_position - angle_position,-MAX_ANGLE_INCLINATION,+MAX_ANGLE_INCLINATION);

   /* Level proportional stabilisation. */
    p_term = (pid->p_gain * err_possition) /1000;

   /*
    * p_dyn_gain->p roll or pitch arround 70
    * Gyro_Rate is +- 24000 is +- 300deg/Sec let fit it to +-8192 +-2000 deg/Sec
    * 300/ 24000 = 0.0125 degree/second  for one bit.
    * 2000/8192  =  0.244 degree/second  for one bit.
    * 0.244/0.0125 ~ 19
    * So 19 is fitting coefficient
    */

    /* Angle speed proportional stabilisation */
    p_term = p_term - (Gyro_Rate * (pid->p_dyn_gain))/ 80 / 19;

    /* Level integral stabilisation */
    pid->i_state = constrain(pid->i_state + err_possition,-10000,10000);
    i_term       =(pid->i_state * pid->i_gain)/4096; /* pid->i_gain arround 13,so resul arround +- 30. */

    /* To found acceleration calculate delta speed. */
    d_state = Gyro_Rate - pid->d_old_gyro_rate;

    /* Save current speed value */
    pid->d_old_gyro_rate = Gyro_Rate;

    /* Something like smoothing filter. */
    d_summ = d_state + pid->d_0_state + pid->d_1_state;
    pid->d_1_state = pid->d_0_state;
    pid->d_0_state = d_state;

    d_term = (d_summ * pid->d_dyn_gain)/2048 ;/* //32// pid->d_gain arround 40 , so pid->d_gain/32 arround 1~2 */

    pid_summ = p_term + i_term - d_term;
    return(pid_summ);
}


int32_t constrain(int32_t x,int32_t y,int32_t z)
{
    if(x<y)
    {
        return(y);
    }
    else if(x>z)
    {
        return(z);
    }
    return(x);
}